#include "splines.h"
#include "utils.h"
#include <cassert>
#include <cmath>
#include <iostream>

//cosas de gauss
typedef vector<vector<double> > mat;

vector<int> gaussElim(mat& a) {

	int n=a.size();
	assert(n>0);
	forn(i,n) assert(int(a[i].size())==n+1);
    vector<int> perm(n); forn(i,n) perm[i]=i;
	forn(i,n) {
		int mi,mj; double ma=0.0;
		forsn(ni,i,n)forsn(nj,i,n) if (fabs(a[ni][nj])>ma) {
			ma=abs(a[ni][nj]); mi=ni; mj=nj;
		}
		if (ma==0.0) {//a is singular
           cout << "GAUSS PANIC" << endl; break;
        } 
        swap(a[i],a[mi]); //swap rows
	    forn(ni,n) swap(a[ni][i],a[ni][mj]); //swap cols
        swap(perm[i],perm[mj]); //update permutation "matrix"
		forsn(ni,i+1,n) {
			forsn(j,i+1,n+1) a[ni][j]-=a[i][j]*a[ni][i]/a[i][i];
			a[ni][i]=0.0;
		}
	}
    return perm;
}

vector<double> solve(mat& a) {
	vector<int> perm = gaussElim(a);
	int n=a.size(); assert(n>0);
	forn(i,n) assert(int(a[i].size())==n+1);
	vector<double> res(n,0.0);
	dforn(i,n) {
		res[i]=a[i][n];
		forsn(j,i+1,n) res[i]-=a[i][j]*res[j];
		res[i]/=a[i][i];
	}
    vector<double> r(n);
    forn(i,n) r[perm[i]]=res[i];
	return r;
}

//resolver splines

#define va(i) (((i)-1)*4+0)
#define vb(i) (((i)-1)*4+1)
#define vc(i) (((i)-1)*4+2)
#define vd(i) (((i)-1)*4+3)
inline double cube(double x) { return x*x*x; }

spline app(vector<double> pts) {
	int n=pts.size();
	spline r(n-1, pol(4));
	
	/*armar sistema de ecuaciones
	 * las variables estan numeradas empezando de 1!!!
	 */
	mat m(4*(n-1), vector<double>(4*(n-1)+1) );
	int cr=4*(n-1);
	//ecuacion 1
	int f=0;
	forsn(i,1,n) {
		m[f][va(i)]=cube(i);
		m[f][vb(i)]=sqr(i);
		m[f][vc(i)]=i;
		m[f][vd(i)]=1;
		m[f][cr]=pts[i-1];
		f++;
	}
	//ecuacion 2
	forsn(i,1,n) {
		m[f][va(i)]=cube(i+1);
		m[f][vb(i)]=sqr(i+1);
		m[f][vc(i)]=i+1;
		m[f][vd(i)]=1;
		m[f][cr]=pts[i];
		f++;
	}
	//ecuacion 3
	forsn(i,1,n-1) {
		m[f][va(i)]=3*sqr(i+1);
		m[f][vb(i)]=2*(i+1);
		m[f][vc(i)]=1;
		m[f][va(i+1)]=-3*sqr(i+1);
		m[f][vb(i+1)]=-2*(i+1);
		m[f][vc(i+1)]=-1;
		m[f][cr]=0.0;
		f++;
	}
	//ecuacion 4
	forsn(i,1,n-1) {
		m[f][va(i)]=6*(i+1);
		m[f][vb(i)]=2;
		m[f][va(i+1)]=-6*(i+1);
		m[f][vb(i+1)]=-2;
		m[f][cr]=0.0;
		f++;
	}
	//ecuacion 5
	m[f][va(1)]=6;
	m[f][vb(1)]=2;
	m[f][cr]=0.0;
	f++;
	//ecuacion 6
	m[f][va(n-1)]=6*n;
	m[f][vb(n-1)]=2;
	m[f][cr]=0.0;
	
	//resolver sistema
	vector<double> rs=solve(m);
	
	//armar resultado
	forsn(i,1,n) {
		r[i-1][3]=rs[va(i)];
		r[i-1][2]=rs[vb(i)];
		r[i-1][1]=rs[vc(i)];
		r[i-1][0]=rs[vd(i)];
	}
	
	return r;
}
